#include "gyro.h"
#include "debug.h"
#include "tool.h"

void DrawBox(RotatedRect &box, Mat img, Rect RoiRect)
{
	Point2f pt[4];
	int i;
	box.points(pt);

#ifdef DRAW_ARMOR	

	line(img, pt[0], pt[1], CV_RGB(0, 255, 255), 3, 8, 0);
	line(img, pt[1], pt[2], CV_RGB(0, 255, 255), 3, 8, 0);
	line(img, pt[2], pt[3], CV_RGB(0, 255, 255), 3, 8, 0);
	line(img, pt[3], pt[0], CV_RGB(0, 255, 255), 3, 8, 0);

#endif

	for (i = 0; i < 4; i++)
	{
		pt[i].x += RoiRect.x;
		pt[i].y += RoiRect.y;
	}
	box.center.x += RoiRect.x;
	box.center.y += RoiRect.y;
	if (box.center.x < 0 || box.center.x > GWIDTH)
	{
		box.center.x = 0;
	}
	if (box.center.y < 0 || box.center.y > GHEIGHT)
	{
		box.center.y = 0;
	} 


}
int Gyro::RoiGyro(float x, float y, float width, float height, int id, Mat frame, Rect RoiRect )
{

	fcount_++;
		
	if (fcount_ == 1)
	{
			
		smallGyro.angle = 0;
		smallGyro.size.height = height / 2;
		smallGyro.size.width = smallGyro.size.height * 2;

		armor_id_ = id;
		center_ = x + width / 2;

		left_x_ = x;
		left_y_ = y;
		right_x_ = x + width;
		right_y_ = y + height;
	}
	else
	{
		
		if (direction_ != MOVELESS)
		{
			smallGyro.center.x = (left_x_ + right_x_) / 2;
			smallGyro.center.y = y + height / 2;
		
			cout << "big_diff:" << right_x_ - smallGyro.center.x << endl;
#ifdef DEBUG
		draw_rect(smallGyro, frame);
		imshow("gyro", frame);
waitKey(1);
#endif
		}
//********************************************************************
		dir temp;
		if ((x + width / 2) - center_ > ISMOVE)
		{
			temp = RIGHT;
		}
		else if (center_ - (x + width / 2) > ISMOVE)
		{
			temp = LEFT;		
		}
		else 
		{temp = MOVELESS;}
		direction_ = GetDirection(temp);

//********************************************************************

		if (direction_ == LEFT && ((x + width / 2) - center_ > BIGDIFF))
		{
			right_x_ = x + width;
			if (x + width > right_x_)
			{
				left_x_ += x + width - right_x_;
			}
			else
			{
				left_x_ -= right_x_ - (x + width);
			}
		}
		else if (direction_ == RIGHT && (center_ - (x + width / 2) > BIGDIFF))
		{
			left_x_ = x;
			if (x  > left_x_)
			{
				right_x_ += x - left_x_;
			}
			else
			{
				right_x_ -= right_x_ - x;
			}
		}
		else
		{
			left_x_ = x < left_x_ ? x : left_x_;
			right_x_ = (x + width) > right_x_ ? (x + width) : right_x_; 
		}
		
		left_y_ = y < left_y_ ? y : left_y_;
		right_y_ = (y + height) > right_y_ ? (y + height) : right_y_;
		


//********************************************************************

		float l_diff = (x + width / 2) - ((left_x_ + right_x_) / 2);
		cout <<l_diff << endl;
		float r_diff = ((left_x_ + right_x_) / 2) - (x + width / 2);


		if (direction_ == LEFT && l_diff > MINFIRE && l_diff < MAXFIRE)
		{
			cout <<l_diff << endl;
			cout << "Fire！" << endl;
			return 1;
		}
		if (direction_ == RIGHT && r_diff > MINFIRE && r_diff < MAXFIRE)
		{
			cout <<r_diff << endl;
			 cout << "Fire！" << endl;
			return 1;
		}


//********************************************************************
		center_ = x + width / 2;
		
		return 0;
	}
}

/* void Gyro::get_Speed(float Pos, float cur_dis)
{
	 float speed = Pos > center_ ? (Pos - center_) : (center_ - Pos);
	cout << speed << endl; 
	
	float speed;

	if(prePosition == 0)
	{
		prePosition = Pos;
        //cout <<"prePosition = Pos;" <<endl;
	}
	else
	{
		speedCount++;
        //cout <<"speedCount++;" <<endl;
		if (speedCount >= 5 && cur_dis != 0)
		{
			speed = prePosition > Pos ? (prePosition - Pos) * cur_dis : (Pos - prePosition) * cur_dis;
			// cout << speed << endl;
			prePosition = Pos;
			speedCount = 0;
		}
	}
} */

void Gyro::Clear()
{
    // speedCount = 0;
	// prePosition = 0;
	fcount_ = 0;
	direction_ = MOVELESS;
	armor_id_ = 0;
	vdirection.clear();
}

dir Gyro::GetDirection(dir direction)
{
	vdirection.push_back(direction);
	
	if (vdirection.size() < JUDGEDIC)
	{
		return MOVELESS;
	}
	else
	{
		int lcount = 0;
		int rcount = 0;
		int mcount = 0;

		for (int i = 0; i < vdirection.size(); i++)
		{
			if (vdirection[i] == RIGHT)
			{
				rcount++;
			}
			else if (vdirection[i] == LEFT)
			{
				lcount++;
			}
		}
		// cout << "count: " << count << endl;
		vdirection.erase(vdirection.begin());
		if(lcount >= JUDGEDIC / 3)
		{
			//cout << "<<<<<<<<<---------" << endl;
			return LEFT;
		}
		else if(rcount >= JUDGEDIC / 3)
		{
			//cout << "--------->>>>>>>>>" << endl;
			return RIGHT;
		}
		else
		{
			//cout << "------------------" << endl;
			return MOVELESS;
		}
	}
	
}

/* void Gyro::get_armor_id(int id)
{
	armor_id_ = id;
} */
